#include "initialize.hpp"
#include "graph.hpp"
#include "move.hpp"
#include "config.hpp"
std::string cfgname,wtsname,camdataname;
int camIndexA,camIndexB;
io_context* io1;
VideoCapture* major_cam;
VideoCapture* sub_cam;
Detector* detector;
float deepbias;
//func to generate undistort pics
std::function<void()> func2 = [](){
	cout<<"major_vision"<<endl;
	while(major_cam->isOpened()){
		if(major_Vision->usable()&&!major_Vision->newPic){
			major_Vision->feedInPic();
			major_Vision->undistortGenerate();
			major_Vision->newPic = 1;
		}	
	}
};
std::function<void()> func1 = [](){
	cout<<"sub_vision"<<endl;
	while(sub_cam->isOpened()){
		if(sub_Vision->usable()&&!sub_Vision->newPic){
			sub_Vision->feedInPic();
			sub_Vision->undistortGenerate();
			sub_Vision->newPic = 1;
		}	
	}
};


//func to initialize everything while throwing out a thread to keep communication
std::function<void(int)> func0 = [](int mode){
	//major handles
	namedWindow("a");
	io1 =new io_context();
	detector = new Detector(cfgname,wtsname);
	try
	{
		major_cam = new VideoCapture(camIndexA);
		sub_cam = new VideoCapture(camIndexB);
	}
	catch(std::exception e){
		cout<<"major_cam:"<<major_cam->isOpened()<<endl
		<<"sub_cam:"<<sub_cam->isOpened()<<endl;
	}
	if(major_cam->isOpened())
		major_Vision = new Vision(major_cam,camdataname,detector);
	if(sub_cam->isOpened())
		sub_Vision = new Vision(sub_cam,camdataname,detector);

	//base control handles
	asio::serial_port serial(*io1,serial_port_name);
	if (!serial.is_open()){
		cout<<"invalid serialport name"<<endl;
	}
	mav::mavio mav(serial);
	mavlink_channel_t chan = MAVLINK_COMM_0;
	mav::bind_mavio_to(chan, mav);
	major_Controller = new base_controller(chan,1,serial);
	mav.setReceivedFunc(std::bind(&base_controller::synchronize,major_Controller,std::placeholders::_1));
	
	//creates base tasks for io1
	asio::steady_timer pixbeater(*io1);
	major_Controller->async_pixHeartBeat(&pixbeater);
	asio::steady_timer msg_timer(*io1);
	//start io1 in another thread
	std::thread netserver([&]() {io1->run();});

	//add a task to check communication, armed in the processure
	major_Controller->check_stream(&msg_timer);
	major_Controller->changeMode(mode);
	// while(major_Controller->droneStatus.sys_status!=MAV_STATE_ACTIVE&&
	// 	major_Controller->droneStatus.sys_status!=MAV_STATE_STANDBY)
	// {
	// 	major_Controller->arm_disarm(1);
	// 	//mavlink_msg_manual_control_send(chan,1,0,0,500,0,0);
	// 	cout<<major_Controller->droneStatus.sys_status<<endl;
	// 	Sleep(500);
	// }
	std::thread picthread_m(func1);
	std::thread picthread_s(func2);
	std::thread ctrlthread(func3);
	
	picthread_m.join();
	picthread_s.join();
	ctrlthread.join();
	netserver.join();
};
